#!/usr/bin/env pybricks-micropython

from pybricks.ev3devices import Motor
from pybricks.parameters import Port,Direction, Color, Button
from pybricks.robotics import DriveBase
from pybricks.parameters import Color, Stop,Direction
from pybricks.ev3devices import TouchSensor,ColorSensor,UltrasonicSensor,InfraredSensor
from pybricks.media.ev3dev import Font
from pybricks.tools import wait, StopWatch
import time

class MyRobot:
    ev3 = None
    EV3_SOUND_VOLUME = 1       #EV3声音大小
    EV3_DRIVE_SPEED = 25       #电机速度
    EV3_DRIVE_ACCU_SPEED = 60  #电机加速度
    # EV3_TRUN_SPEED = 20
    EV3_TRUN_SPEED = 10        #电机转弯转动速度
    EV3_TRUN_ACCU_SPEED = 60   #电机转弯转动加速度

    EV3_CHECK_DRIVE_SPEED = 25       #电机速度
    EV3_CHECK_DRIVE_ACCU_SPEED = 60  #电机加速度
    EV3_CHECK_TRUN_SPEED = 9        #电机转弯转动速度
    EV3_CHECK_TRUN_ACCU_SPEED = 60   #电机转弯转动加速度


    EV3_zhuaGan_SPEED = 500    #电机抓杆转速
    EV3_zhuaZi_SPEED = 500    #电机爪子转速

    ZhuaGuan_DEGREE_MAX  =  17540
    ZhuaGuan_DEGREE_CURR =  17540
    zhuaZi_DEGREE_MAX    =  17540
    zhuaZi_DEGREE_CURR   =  17540

    ROBOT_TASK1_COLOR_CHECK_TIMES         = 2
    ROBOT_TASK1_COLOR_CHECK_TIMES_BLUE    = 10
    ROBOT_TASK1_COLOR_CHECK_DEGREE        = 2
    ROBOT_TASK1_COLOR_CHECK_DEGREE_TIMES  = 5
    ROBOT_TASK1_COLOR_CHECK_DEGREE_OFFSET = -1

    redPosition  = 0
    bluePosition = 0

    G_TURN_DEGREE = 0

    speedOption = 1
    wayOption  = 1
 
    xlog  = 0
    ylog  = 1
    turnOffDegree = 0
    task2_dis1 = 170-50
    timeUsed_group1 = 0
    timeUsed_group2 = 0
    timeUsed_group3 = 0
    timeUsed_group4 = 0
    timeUsed_group5 = 0
    timeUsed_groupTotal = 0
    def __init__(self,ev3, wayOption=1, speedOption=1):
        # Initialize the motors.
        self.speedOption = speedOption
        self.wayOption  =  wayOption
        self.ev3 = ev3
        self.left_motor      = Motor(Port.A)
        self.right_motor     = Motor(Port.B)
        self.zhuaGan_motor   = Motor(Port.C)
        self.zhuaZi_motor    = Motor(Port.D)
        self.switch      = TouchSensor(Port.S2)
        self.colorChecker= ColorSensor(Port.S1) 
        self.ultrasonic  = UltrasonicSensor(Port.S4)
        self.infrared    = InfraredSensor(Port.S3)
        
        if(self.wayOption ==1):
            self.EV3_SOUND_VOLUME = 5 #EV3声音大小
        if(self.wayOption ==2):
            self.EV3_SOUND_VOLUME = 5
        if(self.wayOption ==3):
           self.EV3_SOUND_VOLUME = 100

        self.ev3.speaker.set_speech_options('en','m1',100,100)
        self.ev3.speaker.set_volume(self.EV3_SOUND_VOLUME, '_all_')
        
        # 初始化电机行驶参数.
        self.robot = DriveBase(self.left_motor, self.right_motor, wheel_diameter=10.8, axle_track=205)  
        # 初始化电机行驶参数.
        print("===speedOption" + str(self.speedOption))
        if ( self.speedOption==1):
               print("-1")
               self.EV3_DRIVE_SPEED = 33       #电机速度
               self.EV3_DRIVE_ACCU_SPEED = 70  #电机加速度
               self.EV3_TRUN_SPEED = 15        #电机转弯转动速度
               self.EV3_TRUN_ACCU_SPEED = 70   #电机转弯转动加速度

               self.EV3_zhuaGan_SPEED  = 700    #电机抓杆转速
               self.EV3_zhuaZi_SPEED   = 700

               self.EV3_CHECK_DRIVE_SPEED = 33       #电机速度
               self.EV3_CHECK_DRIVE_ACCU_SPEED = 70  #电机加速度
               self.EV3_CHECK_TRUN_SPEED = 18        #电机转弯转动速度
               self.EV3_CHECK_TRUN_ACCU_SPEED = 70   #电机转弯转动加速度

        if ( self.speedOption==2):
               print("2")
               self.EV3_DRIVE_SPEED = 33       #电机速度
               self.EV3_DRIVE_ACCU_SPEED = 70  #电机加速度
               self.EV3_TRUN_SPEED = 20        #电机转弯转动速度
               self.EV3_TRUN_ACCU_SPEED = 70   #电机转弯转动加速度

               self.EV3_zhuaGan_SPEED  = 700    #电机抓杆转速
               self.EV3_zhuaZi_SPEED   = 700

               self.EV3_CHECK_DRIVE_SPEED = 33       #电机速度
               self.EV3_CHECK_DRIVE_ACCU_SPEED = 70  #电机加速度
               self.EV3_CHECK_TRUN_SPEED = 25        #电机转弯转动速度
               self.EV3_CHECK_TRUN_ACCU_SPEED = 70   #电机转弯转动加速度
        if ( self.speedOption==3):
               print("3")
               self.EV3_DRIVE_SPEED = 45       #电机速度
               self.EV3_DRIVE_ACCU_SPEED = 80  #电机加速度
               self.EV3_TRUN_SPEED = 23        #电机转弯转动速度
               self.EV3_TRUN_ACCU_SPEED = 80   #电机转弯转动加速度

               self.EV3_zhuaGan_SPEED  = 800    #电机抓杆转速
               self.EV3_zhuaZi_SPEED   = 800

               self.EV3_CHECK_DRIVE_SPEED = 45       #电机速度
               self.EV3_CHECK_DRIVE_ACCU_SPEED = 90  #电机加速度
               self.EV3_CHECK_TRUN_SPEED = 28        #电机转弯转动速度
               self.EV3_CHECK_TRUN_ACCU_SPEED = 90   #电机转弯转动加速度

        self.robot.settings(self.EV3_DRIVE_SPEED,self.EV3_DRIVE_ACCU_SPEED,self.EV3_TRUN_SPEED,self.EV3_TRUN_ACCU_SPEED)
        # Initialize variable  
        self.ev3.screen.clear()
  
    def addLog(self,input):
        print(input)
        self.ev3.screen.print(input)
    
    def paySound(self,sound):
        self.ev3.speaker.play_notes(sound)

    def goAhead(self, distance=0):
        self.addLog("-- goAhead  -- " + str(distance) + " cm")
        self.robot.straight(distance)
    
    def goAheadWithAcc(self, distance=0):
         
        tmp_EV3_DRIVE_SPEED = 45       #电机速度
        tmp_EV3_DRIVE_ACCU_SPEED = 45  #电机加速度
        tmp_EV3_TRUN_SPEED = 15        #电机转弯转动速度
        tmp_EV3_TRUN_ACCU_SPEED = 60   #电机转弯转动加速度
        self.robot.stop()
        self.robot.settings(tmp_EV3_DRIVE_SPEED,tmp_EV3_DRIVE_ACCU_SPEED,tmp_EV3_TRUN_SPEED,tmp_EV3_TRUN_ACCU_SPEED)
        self.addLog("-- goAhead  -- " + str(distance) + " cm")
        self.robot.straight(distance)  
        self.robot.stop()    
        self.robot.settings(self.EV3_DRIVE_SPEED,self.EV3_DRIVE_ACCU_SPEED,self.EV3_TRUN_SPEED,self.EV3_TRUN_ACCU_SPEED)

    def goDrive(self,speed=25):
        self.addLog("goDrive  ------------")
        self.robot.drive(speed,0)

    def goDriveStop(self):
        self.addLog("goDriveStop  ------------")
        self.robot.stop()     

    def turn(self, degree):
        self.addLog("#后偏离角度："+str(degree))
        self.robot.turn(degree)
        self.turnOffDegree = self.turnOffDegree + degree
        self.addLog("#当前总偏离角度："+str( self.turnOffDegree ))

    def zhuaGanDownByDegree(self,speed=10, angle =0):
        if speed < 0:
            speed = speed * - 1
        currentAngle = self.zhuaGan_motor.angle()
        targeAngle = currentAngle +  angle
        self.zhuaGan_motor.run(speed)  
        self.addLog("current"+str(currentAngle))
        self.addLog("targeAngle"+str(targeAngle))
        print("frank1")
        while (self.zhuaGan_motor.angle()<targeAngle):
        #    self.addLog("true")
           time.sleep(0.01)
        self.zhuaGan_motor.stop()
        self.addLog("motor degree:"+str(self.zhuaGan_motor.angle()))
    
    def zhuaGanUpByDegree(self,speed=10,angle =0):
        if speed > 0:
            speed = speed * - 1

        currentAngle = self.zhuaGan_motor.angle()
        targeAngle = currentAngle -  angle
        self.zhuaGan_motor.run(speed)  
        self.addLog("current"+str(currentAngle))
        self.addLog("targeAngle"+str(targeAngle))
        print("frank1")
        print(self.zhuaGan_motor.angle())
        while (targeAngle < self.zhuaGan_motor.angle() and not(self.switch.pressed())):
        #    self.addLog("true")
           time.sleep(0.01)
        self.zhuaGan_motor.stop()
        self.addLog("motor degree:"+str(self.zhuaGan_motor.angle()))

    def zhuaGanUpToTop(self,speed=10):
        if speed > 0:
            speed = speed * - 1   
        self.zhuaGan_motor.run(speed)  
        self.addLog("start")
        while not(self.switch.pressed()):
        #    self.addLog("true")
           time.sleep(0.5)
        self.zhuaGan_motor.stop()
  
    def zhuaZiOpen(self,speed=20, degree =0):
        self.addLog("zhua zi Open  ------------")
        if speed < 0:
            speed = speed * - 1
        currentAngle = self.zhuaZi_motor.angle()
        targeAngle = currentAngle +  degree
        self.zhuaZi_motor.run(speed)  
        self.addLog("current"+str(currentAngle))
        self.addLog("targeAngle"+str(targeAngle))
        while (self.zhuaZi_motor.angle()<targeAngle):
           time.sleep(0.01)
        self.zhuaZi_motor.stop()
        self.addLog("motor degree:"+str(self.zhuaZi_motor.angle()))

    def zhuaZiClose(self,speed=20, degree =0):
        self.addLog("zhua Zi Close  ------------")
        if speed > 0:
            speed = speed * - 1
        currentAngle = self.zhuaZi_motor.angle()
        targeAngle = currentAngle -  degree
        self.zhuaZi_motor.run(speed)  
        self.addLog("current"+str(currentAngle))
        self.addLog("targeAngle"+str(targeAngle))
        while (self.zhuaZi_motor.angle()>targeAngle):
           time.sleep(0.01)
        self.zhuaZi_motor.stop()
        self.addLog("motor degree:"+str(self.zhuaZi_motor.angle()))

    def checkDisLimist(self, degree):
        MAX_DIS = 500
        MIN_DIS = 200
        if degree>MAX_DIS:
            return MAX_DIS
        if degree<MIN_DIS:
            return MIN_DIS       
        return degree 

    def checkObjectByDegree(self,degree):
        self.addLog("check Object ------------")
        MAX_DIS = 500
        MIN_DIS = 200

        currDistance = self.ultrasonic.distance()
        self.turn(degree)
        newDistance = self.ultrasonic.distance()
        self.addLog('Degree:'+str(degree)+' CurrDis:'+str(currDistance)+' NewDis:'+str(newDistance))
        if (self.checkDisLimist(currDistance) <= self.checkDisLimist(newDistance)):
            self.addLog("Cup is in current")
            return True
        else:
            self.addLog("Cup is in newDistance")
            return False
    
    def checkObjectByLargeDegree(self,startDegree,endDegree,stepDegree,offset=0):
        self.addLog("checkObjectByLargeDegree---start-1--")
        resList=[]
        num = (endDegree-startDegree) / stepDegree
        self.turn(startDegree)
        resList.append(self.ultrasonic.distance())
        i=1
        while i < num :
        #    self.addLog("check Distance "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+(i*stepDegree)))
           self.turn(stepDegree )
           resList.append(self.ultrasonic.distance())
           i=i+1
        lastDeg=(endDegree - (startDegree+ (i-1)*stepDegree))
        # self.addLog("check Distance "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+ (i-1)*stepDegree+lastDeg))
        self.turn(lastDeg)
        resList.append(self.ultrasonic.distance())
 
        minIndex = resList.index(min(resList))
        # self.addLog("checkObjectByLargeDegree---start-2--")
        self.addLog("minIndex is:" + str(minIndex))
        self.addLog(resList)
        reCheck=[]
        ind=1
        resIndex=minIndex
        while ind< len(resList):
            if (resList[ind]==resList[minIndex]):
                reCheck.append(ind)
            ind=ind+1
        self.addLog("reCheck:")
        self.addLog(reCheck)
        if len(reCheck)>1:
              temp=int(len(reCheck)/2)
              resIndex = temp
        self.addLog("resIndex "+ str(resIndex))   
        if(resIndex < i):
            lastTurnDeg=(lastDeg+ (i-resIndex-1)*stepDegree - offset)*-1 
            self.addLog("turn to the resIndex degree "+ str(lastTurnDeg))
            self.turn(lastTurnDeg)
        else:
            self.addLog("No need turn since current is resIndex")
     
        return resList[resIndex]

    def checkColorByLargeDegree(self,checkColor, startDegree,endDegree,stepDegree,offset=0):
        self.addLog("checkColorByLargeDegree---start-1--")
        #1.根据角度计算转向的次数
        resList=[]
        num = (endDegree-startDegree) / stepDegree
        self.turn(startDegree)
        resList.append(self.colorChecker.color())
        i=1
        #2.依次侧旋转，测量颜色
        while i < num :
           self.addLog("check color "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+(i*stepDegree)))
           self.turn(stepDegree )
           resList.append(self.colorChecker.color())
           i=i+1
        lastDeg=(endDegree - (startDegree+ (i-1)*stepDegree))
        self.addLog("check color "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+ (i-1)*stepDegree+lastDeg))
        self.turn(lastDeg)
        resList.append(self.colorChecker.color())
 
        #3.检查测量的颜色是不是想要的，resIndex是最终检测到的下标
        self.addLog("checkObjectByLargeDegree---start-2--")
        self.addLog(resList)
        checkInd=0
        reCheck=[]
        resIndex=0
        ind=0
        while checkInd< len(resList):
            if (str(resList[checkInd])==str(checkColor)):
                reCheck.append(checkInd)
            checkInd=checkInd+1
        self.addLog("reCheck:")
        self.addLog(reCheck)
        if len(reCheck)>1:
              temp=int(len(reCheck)/2)
              resIndex = reCheck[temp]
        else:
              temp=int( len(resList)/2)
              resIndex=temp

        self.addLog("resIndex "+ str(resIndex))  
        #4.根据下标，调整到对应的角度
        if(resIndex < i):
            lastTurnDeg=(lastDeg+ (i-resIndex-1)*stepDegree - offset)*-1 
            self.addLog("turn to the resIndex degree "+ str(lastTurnDeg))
            self.turn(lastTurnDeg)
        else:
            self.addLog("No need turn since current is resIndex")
         
        #5.返回找不找到
        if len(reCheck)>1:
            return True
        else:
            return False

    #扫面给定的角度和颜色，返回是否找到颜色，和转向的偏离角度。
    def scanColorByLargeDegree(self,checkColor, startDegree,endDegree,sleepTime=0.01,offset=0):
        self.addLog("scanColorByLargeDegree---start-1--")
        #1.根据角度计算转向的次数
        resList=[]
        resListDeg=[]

        self.robot.stop()
        self.robot.settings(self.EV3_CHECK_DRIVE_SPEED,self.EV3_CHECK_DRIVE_ACCU_SPEED,self.EV3_CHECK_TRUN_SPEED,self.EV3_CHECK_TRUN_ACCU_SPEED )
        self.robot.turn(startDegree)
        

        currentTurn =  self.robot.angle()
        self.robot.drive(0, self.EV3_CHECK_TRUN_SPEED)
        while (self.robot.angle()-currentTurn)< (endDegree-startDegree) :
            resListDeg.append((self.robot.angle()-currentTurn))
            resList.append(self.colorChecker.color())
            time.sleep(sleepTime)
            print("--------------")
  
        resListDeg.append((self.robot.angle()-currentTurn))
        resList.append(self.colorChecker.color())
        self.addLog(resListDeg)
        self.addLog("len of resListDeg:" + str(len(resListDeg)))
        self.addLog(resList)
        self.addLog("len of resList:" + str(len(resList)))
 
        #3.检查测量的颜色是不是想要的，resIndex是最终检测到的下标
        self.addLog("checkObjectByLargeDegree---start-2--")
        checkInd=0
        reCheck=[]
        resIndex=0
        ind=0
        while checkInd< len(resList):
            if (str(resList[checkInd])==str(checkColor)):
                reCheck.append(checkInd)
            checkInd=checkInd+1
        self.addLog("reCheck:")
        self.addLog(reCheck)
        if len(reCheck)>1:
              temp=int(len(reCheck)/2)
              resIndex = reCheck[temp]
        else:
              temp=int( len(resList)/2)
              resIndex=temp

        self.addLog("resIndex "+ str(resIndex))  
        #4.根据下标，调整到对应的角度
       
        turnDegree =((endDegree - startDegree) - resListDeg[resIndex])  
        self.robot.turn((turnDegree+ offset) * -1)
        self.addLog("robot offset "+ str(endDegree- turnDegree))  
        #调整因转弯产生的后移
        self.goAhead(5)
        #5.返回找不找到
        if len(reCheck)>1:
            self.addLog("scanColorByLargeDegree  find color")  
            return True, endDegree- turnDegree
        else:
            self.addLog("scanColorByLargeDegree  not find color")  
            return False,endDegree- turnDegree 


    def checkObjectByLargeDegreeNew2(self,startDegree=-10,endDegree=10,stepDegree=5,sleepTime=0.1,offset=0):
        maxLimit = 1000

        self.robot.stop()
        self.robot.settings(self.EV3_CHECK_DRIVE_SPEED,self.EV3_CHECK_DRIVE_ACCU_SPEED,self.EV3_CHECK_TRUN_SPEED,self.EV3_CHECK_TRUN_ACCU_SPEED )

        self.addLog("checkObjectByLargeDegreeNew2---start-1--")
        resListDeg=[]
        resListDis=[]
        # num = (endDegree-startDegree) / stepDegree
        self.robot.turn(startDegree)
        # self.robot.reset()
        currentTurn =  self.robot.angle()
        self.robot.drive(0, self.EV3_CHECK_TRUN_SPEED)
        while (self.robot.angle()-currentTurn)< (endDegree-startDegree) :
            # self.addLog("dis:" + str(self.robot.distance()))
            # self.addLog("deg:" + str((self.robot.angle()-currentTurn)))
            resListDeg.append((self.robot.angle()-currentTurn))
            resListDis.append(self.ultrasonic.distance())
            time.sleep(sleepTime)
        if(self.EV3_CHECK_TRUN_SPEED > 13):
            self.EV3_CHECK_TRUN_SPEED = self.EV3_CHECK_TRUN_SPEED - 3
        resListDeg.append((self.robot.angle()-currentTurn))
        resListDis.append(self.ultrasonic.distance())
        self.addLog(resListDeg)
        self.addLog(resListDis)
        res,disres = self.find_max_consecutive_indices(resListDeg,resListDis,2550)
        self.addLog(res)
         
        turnDeg = (endDegree-startDegree - res)* -1 + offset
        self.addLog("turn deg:"+ str(turnDeg))
        self.robot.turn( turnDeg)
        self.G_TURN_DEGREE = self.G_TURN_DEGREE + (turnDeg * -1) - endDegree
        self.turnOffDegree = self.turnOffDegree + (turnDeg * -1) - endDegree

        self.robot.stop()    
        self.robot.settings(self.EV3_DRIVE_SPEED,self.EV3_DRIVE_ACCU_SPEED,self.EV3_TRUN_SPEED,self.EV3_TRUN_ACCU_SPEED)    
        self.addLog ('self.turnOffDegree:'+ str(self.turnOffDegree))

        return disres
 

    def checkObjectByLargeDegreeNew(self,startDegree=-10,endDegree=10,stepDegree=5,sleepTime=0.1,offset=0):
        maxLimit = 1000

        self.robot.stop()
        self.robot.settings(self.EV3_CHECK_DRIVE_SPEED,self.EV3_CHECK_DRIVE_ACCU_SPEED,self.EV3_CHECK_TRUN_SPEED,self.EV3_CHECK_TRUN_ACCU_SPEED )

        self.addLog("checkObjectByLargeDegree---start-1--")
        resListDeg=[]
        resListDis=[]
        # num = (endDegree-startDegree) / stepDegree
        self.robot.turn(startDegree)
        # self.robot.reset()
        currentTurn =  self.robot.angle()
        self.robot.drive(0, self.EV3_CHECK_TRUN_SPEED)
        while (self.robot.angle()-currentTurn)< (endDegree-startDegree) :
            # self.addLog("dis:" + str(self.robot.distance()))
            # self.addLog("deg:" + str((self.robot.angle()-currentTurn)))
            resListDeg.append((self.robot.angle()-currentTurn))
            resListDis.append(self.ultrasonic.distance())
            time.sleep(sleepTime)
        if(self.EV3_CHECK_TRUN_SPEED > 13):
            self.EV3_CHECK_TRUN_SPEED = self.EV3_CHECK_TRUN_SPEED - 3
        resListDeg.append((self.robot.angle()-currentTurn))
        resListDis.append(self.ultrasonic.distance())
        # self.addLog(resListDeg)
        # self.addLog(resListDis)
        res,disres = self.find_longest_consecutive_min_indices(resListDeg,resListDis)
        self.addLog(res)
        if (disres > maxLimit):
            #turn to orignial direct
            self.addLog("遇到最大值，转回原来方向")
            self.robot.turn( endDegree * -1)
        else:
            turnDeg = (endDegree-startDegree - res)* -1 + offset
            self.addLog("turn deg:"+ str(turnDeg))
            self.robot.turn( turnDeg)
            self.G_TURN_DEGREE = self.G_TURN_DEGREE + (turnDeg * -1) - endDegree
            self.turnOffDegree = self.turnOffDegree + (turnDeg * -1) - endDegree
        self.robot.stop()    
        self.robot.settings(self.EV3_DRIVE_SPEED,self.EV3_DRIVE_ACCU_SPEED,self.EV3_TRUN_SPEED,self.EV3_TRUN_ACCU_SPEED)    
        self.addLog ('self.turnOffDegree:'+ str(self.turnOffDegree))

        return disres
        
    def find_max_consecutive_indices(self,degList,listDis, max_value):  
        if not listDis or max_value not in listDis:  
            return []  
        
        max_consecutive_indices = []  # 用于存储最长的连续索引序列  
        current_indices = []         # 用于存储当前的连续索引序列  
        current_length = 0           # 当前连续长度  
        max_length = 0               # 最长连续长度  
    
        for i, value in enumerate(listDis):  
            if value == max_value:  
                current_indices.append(i)  
                current_length += 1  
                if current_length > max_length:  
                    max_length = current_length  
                    max_consecutive_indices = current_indices.copy()  
            else:  
                # 当遇到非最大值时，重置当前连续索引序列和长度  
                current_indices = []  
                current_length = 0  
        
        self.addLog("max_consecutive_indices")
        self.addLog(max_consecutive_indices)

        if ((len(max_consecutive_indices) % 2 == 0)/2):
            self.addLog('偶数')
            ind = int(len(max_consecutive_indices)  / 2 - 1)

            return int((degList[max_consecutive_indices[ind]] +  degList[max_consecutive_indices[ind+1]]) / 2) ,int((listDis[max_consecutive_indices[ind]] +  listDis[max_consecutive_indices[ind+1]]) / 2)
        else:
            self.addLog('奇数')
            ind = int( (len(max_consecutive_indices) + 1 ) / 2) - 1
            return degList[max_consecutive_indices[ind]], listDis[max_consecutive_indices[ind]]


        
    def find_longest_consecutive_min_indices(self,degList, lst):  
        if len(lst) < 2:  
            return null,null  # 如果列表长度小于2，返回空列表  
        
        min_val = min(lst)  # 找到列表中的最小值  
        max_consecutive_length = 0  # 最长连续序列的长度  
        max_consecutive_indices = []  # 最长连续序列的索引  
        current_length = 0  # 当前连续序列的长度  
        current_indices = []  # 当前连续序列的索引  
        
        for i, val in enumerate(lst):  
            if val == min_val:  
                current_length += 1  # 当前连续序列长度加1  
                current_indices.append(i)  # 添加当前索引到当前连续序列索引列表  
                
                # 如果当前连续序列长度超过了之前记录的最长长度，则更新最长长度和索引列表  
                if current_length > max_consecutive_length:  
                    max_consecutive_length = current_length  
                    max_consecutive_indices = current_indices  
            else:  
                # 当遇到非最小值时，重置当前连续序列的长度和索引列表  
                current_length = 0  
                current_indices = []  
        self.addLog("max_consecutive_indices")
        self.addLog(max_consecutive_indices)

        if ((len(max_consecutive_indices) % 2 == 0)/2):
            self.addLog('偶数')
            ind = int(len(max_consecutive_indices)  / 2 - 1)

            return int((degList[max_consecutive_indices[ind]] +  degList[max_consecutive_indices[ind+1]]) / 2) ,int((lst[max_consecutive_indices[ind]] +  lst[max_consecutive_indices[ind+1]]) / 2)
        else:
            self.addLog('奇数')
            ind = int( (len(max_consecutive_indices) + 1 ) / 2) - 1

            return degList[max_consecutive_indices[ind]], lst[max_consecutive_indices[ind]]
        
    def putThingDownAndUp(self):
        self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*6)
        self.zhuaZiOpen(self.EV3_zhuaZi_SPEED ,180*4+800)
        # self.ev3.speaker.play_file('bigger.wav')
        self.paySound(['C4/8','D4/8','E4/8'])
        self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)
        self.zhuaZiOpen(self.EV3_zhuaZi_SPEED ,180*8+90+5-800)
    

    def scanAndGo3(self, disPeiord=225):
        self.addLog("#scanAndGo3 A 穿越障碍墙体") 

        MAX_LIMIT_DIS = 1000
        startDegree=-45
        endDegree=45
        stepDegree=21
        sleepTime=0.01
        offset=0
        count= 0

        cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
        self.addLog("#scanAndGo 当前最小距离1："+str(cupDis)) 

        while cupDis>190:
           goDis= (cupDis - 190)
           if goDis >= MAX_LIMIT_DIS :
              self.goAheadWithAcc(600+18)
           elif goDis >= 100 and goDis <MAX_LIMIT_DIS:
             self.goAheadWithAcc(goDis  * 3/4 +18)
           elif goDis >= 30  and  goDis < 100:
             self.goAhead(goDis * 3/4 +18)
           else:
             self.goAhead(10+20)
        #    self.goAhead(18)
           count = count + 1
           self.addLog("当前查询次数:"+ str(count))
           if (count <=4):
                startDegree = startDegree + 7
                endDegree = endDegree - 7
                stepDegree = stepDegree - 7
           cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
           self.addLog("#scanAndGo 当前最小距离2:" + str(cupDis))

        self.addLog("#scanAndGo 距离水杯还有1 :"+ str(self.ultrasonic.distance()))
        lastDis = self.ultrasonic.distance() - disPeiord
        self.addLog("##scanAndGo  调整距离:"+str(lastDis))
        self.goAhead(lastDis)
        self.addLog("#scanAndGo 距离水杯还有2 :"+ str(self.ultrasonic.distance()))
        self.turn(2)

    def scanAndGo2(self, disPeiord=225):
        self.addLog("#scanAndGo A 扫描位置") 
        MAX_LIMIT_DIS = 1000
        startDegree=-45
        endDegree=45
        stepDegree=21
        sleepTime=0.01
        offset=0
        count= 0

        cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
        self.addLog("#scanAndGo 当前最小距离1："+str(cupDis)) 

        while cupDis>190:
           goDis= (cupDis - 190)
           if goDis >= MAX_LIMIT_DIS :
              self.goAheadWithAcc(600+18)
           elif goDis >= 100 and goDis <MAX_LIMIT_DIS:
             self.goAheadWithAcc(goDis  * 3/4 +18)
           elif goDis >= 30  and  goDis < 100:
             self.goAhead(goDis * 3/4 +18)
           else:
             self.goAhead(10+20)
        #    self.goAhead(18)
           count = count + 1
           self.addLog("当前查询次数:"+ str(count))
           if (count <=4):
                startDegree = startDegree + 7
                endDegree = endDegree - 7
                stepDegree = stepDegree - 7
           cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
           self.addLog("#scanAndGo 当前最小距离2:" + str(cupDis))

        self.addLog("#scanAndGo 距离水杯还有1 :"+ str(self.ultrasonic.distance()))
        lastDis = self.ultrasonic.distance() - disPeiord
        self.addLog("##scanAndGo  调整距离:"+str(lastDis))
        self.goAhead(lastDis)
        self.addLog("#scanAndGo 距离水杯还有2 :"+ str(self.ultrasonic.distance()))
        self.turn(2)


    def scanAndGo(self, disPeiord=225):
        self.addLog("#scanAndGo A 扫描位置") 

        self.goAheadWithAcc(400)

        MAX_LIMIT_DIS = 1000
        startDegree=-45
        endDegree=45
        stepDegree=21
        sleepTime=0.01
        offset=0
        count= 0

        cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
        self.addLog("#scanAndGo 当前最小距离1："+str(cupDis)) 

        while cupDis>190:
           goDis= (cupDis - 190)
           if goDis >= MAX_LIMIT_DIS :
              self.goAheadWithAcc(600+18)
           elif goDis >= 100 and goDis <MAX_LIMIT_DIS:
             self.goAheadWithAcc(goDis  * 3/4 +18)
           elif goDis >= 30  and  goDis < 100:
             self.goAhead(goDis * 3/4 +18)
           else:
             self.goAhead(10+20)
        #    self.goAhead(18)
           count = count + 1
           self.addLog("当前查询次数:"+ str(count))
           if (count <=4):
                startDegree = startDegree + 7
                endDegree = endDegree - 7
                stepDegree = stepDegree - 7
           cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
           self.addLog("#scanAndGo 当前最小距离2:" + str(cupDis))

        self.addLog("#scanAndGo 距离水杯还有1 :"+ str(self.ultrasonic.distance()))
        lastDis = self.ultrasonic.distance() - disPeiord
        self.addLog("##scanAndGo  调整距离:"+str(lastDis))
        self.goAhead(lastDis)
        self.addLog("#scanAndGo 距离水杯还有2 :"+ str(self.ultrasonic.distance()))
        self.turn(2)

    def goTask1_Group1(self):
        nowStart = time.time()
        self.addLog("GoTask1_Group1 开始")
        self.addLog("1.初始化,抬起抓杆")
        self.paySound(['C4/4'])
        self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)
        self.addLog("#2.后退")
        self.goAhead(-200)
        self.addLog("#3.转弯")
        # self.turn(19*2)
        self.turn(45)
        self.addLog("#4.后退")
        self.goAhead(-400)
        self.addLog("#5.转弯 面向杯子")
        # self.turn(19*2+5)
        self.turn(45+14)
        # self.addLog("#6.调整角度")
        # self.checkObjectByLargeDegree(-4,4,4,0)
        self.addLog("#7.前进，找第一个硬币")
        self.goAhead(95)
        self.addLog("#8. 放下抓杆")
        self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*15 + 160)
        self.addLog("#9. 后退调整位置")
        self.goAhead(-20)
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group1 =round(time_interval,2)
        self.addLog("#goTask1_Group1 用时:"+ str(self.timeUsed_group1) )

    def goTask1_Group2(self):
        nowStart = time.time()
        self.addLog("#9.检查是否是红色")
        self.paySound(['D4/4'])
        self.redPosition = 0
        self.bluePosition = 0
        
        # self.robot.settings(20,30,20,30)

        foundRed = False
        goCheckCount = 0
        count = 0
        accumulatedLenght = 0

        while (goCheckCount < self.ROBOT_TASK1_COLOR_CHECK_TIMES and not(foundRed)):
            goCheckCount = goCheckCount + 1
            while (count < 1) and not(foundRed):
                startDegree = self.ROBOT_TASK1_COLOR_CHECK_DEGREE * (self.ROBOT_TASK1_COLOR_CHECK_DEGREE_TIMES - goCheckCount) * -1 
                endDegree =  self.ROBOT_TASK1_COLOR_CHECK_DEGREE * (self.ROBOT_TASK1_COLOR_CHECK_DEGREE_TIMES - goCheckCount)  
                if (self.checkColorByLargeDegree(Color.RED,startDegree,endDegree,self.ROBOT_TASK1_COLOR_CHECK_DEGREE,self.ROBOT_TASK1_COLOR_CHECK_DEGREE_OFFSET)):
                    self.addLog( "found Red")
                    
                    foundRed = True
                    self.redPosition = 1
                    self.bluePosition = 2
                    break
                count = count + 1
            if (not(foundRed)):
                self.goAhead(35)
                accumulatedLenght = accumulatedLenght +35
                self.addLog (" 1 accumulatedLenght :"+ str(accumulatedLenght))
            count=0

       
        if (foundRed and self.redPosition==1) :
                self.addLog("#10.A 找到红色在位置1上,抓取")
                self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
                self.goAhead(-180)
                self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180+90)
                self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
                # self.ev3.speaker.play_file('jump.wav')
                self.paySound(['C4/8','D4/8','E4/8'])
                self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)
                # self.checkObjectByLargeDegree(-4,4,4,0)
                # Option3是保险模式，检查多
                if (self.wayOption == 3 ):
                    self.checkObjectByLargeDegreeNew(startDegree=-20,endDegree=20,stepDegree=10,sleepTime=0.2,offset=0)
                self.goAhead(380)
                
        else:          
          self.addLog("#10.B 在位置2上是红色")
          self.redPosition = 2
          self.bluePosition = 1
    
          #self.goAhead((180 - 5) * -1 )
          self.goAhead(-50)
          self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+90)
          self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+40)
        #   self.ev3.speaker.play_file('jump.wav')
          self.paySound(['C4/8','D4/8','E4/8'])
          self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)
          self.checkObjectByLargeDegreeNew()
          self.goAhead(100 + (180 - accumulatedLenght))
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group2 =round(time_interval,2)
        self.addLog("#goTask1_Group2 用时:"+ str(self.timeUsed_group2) )
    
    def goTask1_Group3(self):
        nowStart = time.time()
        self.paySound(['E4/4'])
        self.addLog("#11 前往水杯")
        # cupDis=self.checkObjectByLargeDegreeNew(-20,20,5,0)
        # cupDis=self.checkObjectByLargeDegree(-10,10,8)
        cupDis =self.ultrasonic.distance()
        self.addLog("cupDis1:" + str(cupDis))

        startDegree=-9
        endDegree=9
        stepDegree=9
        sleepTime=0.01
        offset=3
        count= 0
        while cupDis>190:
           goDis= (cupDis - 190)
           if goDis > 5:
             self.goAhead(int(goDis * 2/3))
           else:
             self.goAhead(5)
           
           if (self.wayOption==3 or self.wayOption==2):
             self.goAhead(15)
           else:
             self.goAhead(20)

           cupDis=self.checkObjectByLargeDegreeNew(startDegree,endDegree,stepDegree,sleepTime,offset)
           offset = offset -1
           startDegree = startDegree - 3
           endDegree   = endDegree + 3
           self.addLog("cupDis2:" + str(cupDis))
        self.addLog("# 距离水杯还有 :"+ str(self.ultrasonic.distance()))
        lastDis = cupDis - 220
        self.addLog("# 调整距离:"+str(lastDis))
        self.goAhead(lastDis)
        
        

        self.addLog("#12 放下红色到水杯")  
        self.putThingDownAndUp()
        self.addLog("#12.A 调整方向") 
        self.addLog("调整方向:"+str(self.G_TURN_DEGREE* -1)) 
        self.robot.turn(self.G_TURN_DEGREE )
        
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group3 =round(time_interval,2)
        self.addLog("#goTask1_Group3 用时:"+ str(self.timeUsed_group3) )

    def goTask1_Group4(self):
        nowStart = time.time()
        self.paySound(['F4/4'])
        self.addLog("#13 后退，找到蓝色")  
       
        if self.bluePosition == 2 :
            self.addLog("#14 蓝色在第二位置")  
            self.goAhead(-160)
  
        
            foundBlue2 = False
            goCheckCount = 0
            count = 0
            accumulatedLenght = 0

            if (self.wayOption==3):
                while (goCheckCount < self.ROBOT_TASK1_COLOR_CHECK_TIMES_BLUE and not(foundBlue2)):
                    goCheckCount = goCheckCount + 1
                    while (count < 1) and not(foundBlue2):
                        startDegree = self.ROBOT_TASK1_COLOR_CHECK_DEGREE * (self.ROBOT_TASK1_COLOR_CHECK_DEGREE_TIMES + goCheckCount) * -1 
                        endDegree =  self.ROBOT_TASK1_COLOR_CHECK_DEGREE * (self.ROBOT_TASK1_COLOR_CHECK_DEGREE_TIMES  + goCheckCount)  
                        if (self.checkColorByLargeDegree(Color.BLUE,startDegree,endDegree,self.ROBOT_TASK1_COLOR_CHECK_DEGREE,self.ROBOT_TASK1_COLOR_CHECK_DEGREE_OFFSET)):
                            self.addLog( "found blue")
                            self.paySound(['B4/4','B4/4'])
                            foundBlue2 = True                        
                            break
                        count = count + 1
                    if (not(foundBlue2)):
                        self.goAhead(-7)
                    count=0
            self.goAhead(-160)
            self.checkObjectByLargeDegreeNew()
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*14 + 160+90)
            # self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
            
            # self.robot.turn(7)
            # self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180+90)
            self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
            # self.ev3.speaker.play_file('jump.wav')
            self.paySound(['C4/8','D4/8','E4/8'])
            self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)    
            self.goAhead(300)
        else:
            self.addLog("#14 蓝色在第一位置")  
            self.goAhead(-400)
            self.robot.turn(7)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*15 + 160 )
            self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90)
            # self.ev3.speaker.play_file('jump.wav')
            self.paySound(['C4/8','D4/8','E4/8'])
            self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED * -1)
            self.goAhead(380)
        
        self.addLog("#15 转身面向任务二")  
        self.turn(90+20) 
        # self.goTask1_Group5()
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group4 =round(time_interval,2)
        self.addLog("#goTask1_Group4 用时:"+ str(self.timeUsed_group4) )

    def goTask1_Group5(self):
        nowStart = time.time()
        self.scanAndGo()
        self.putThingDownAndUp()
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group5 =round(time_interval,2)
        self.addLog("#goTask1_Group5 用时:"+ str(self.timeUsed_group5) )
       
    def goTask1(self):
        self.addLog("GoTask1 start---") 
        nowStart = time.time()   
        self.goTask1_Group1()
        self.goTask1_Group2()
        self.goTask1_Group3()
        self.goTask1_Group4()           
        self.goTask1_Group5()           
        self.addLog("GoTask1 Finish---")
        # self.ev3.speaker.play_file('complete.wav')
        self.paySound(['C4/8','D4/8','E4/8','F4/8','G4/8','A4/8'])
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_groupTotal = round(time_interval,2)
        self.addLog("#GoTask1 总用时:"+ str(self.timeUsed_groupTotal)) 
        self.addLog("#        Group1:"+ str(self.timeUsed_group1)) 
        self.addLog("#        Group2:"+ str(self.timeUsed_group2))
        self.addLog("#        Group3:"+ str(self.timeUsed_group3))
        self.addLog("#        Group4:"+ str(self.timeUsed_group4))
        self.addLog("#        Group5:"+ str(self.timeUsed_group5))


    def goTask2_Group1(self):
        nowStart = time.time()
        self.addLog("GoTask2_Group1 开始")
        self.addLog("1.初始化,抬起抓杆")
        self.paySound(['C4/4'])
        self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)
        self.addLog("#2.前进")
        
        startDis =0 
        # startDis  = 17 * 6   #如果机器人起始位置是在出发区的后面，需要加入这个前进的距离。
        self.goAhead(300 + startDis) 
        self.addLog("#3. 放下抓杆")
        self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*14+90 )
        
        self.turn(30)
 
        self.addLog("#4.检查是否是红色")
        self.paySound(['D4/4'])
        self.redPosition = 0
        self.bluePosition = 0
        foundRed = False
        res,offset = self.scanColorByLargeDegree(Color.RED,-10,10,0.0625,8)
       
        self.addLog("#4.检查是红色后偏离角度："+str(offset))
        self.turnOffDegree = self.turnOffDegree + offset
        self.addLog("#.当前总偏离角度："+str( self.turnOffDegree ))
        if (res):
                self.addLog("4.1 找到红色在位置1上,抓取")
                self.redPosition =  1
                self.bluePosition = 2

                self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
                self.goAhead(self.task2_dis1 * -1)
                self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180)
                self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
                # self.ev3.speaker.play_file('jump.wav')
                self.paySound(['C4/8','D4/8','E4/8'])
                self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)               
                
                self.addLog("#4.2 回正")
                self.turn(self.turnOffDegree * -1)
                self.addLog("#4.3 转身面向水杯")
                self.turn(-55-10-5-3+10)
                self.goAhead(170-5-3-10-15)
 
                cupDis=self.checkObjectByLargeDegreeNew(-20,20,5,0.005,-2)
                cupDis=self.checkObjectByLargeDegreeNew(-20,20,5,0.015,-2)
                self.addLog("#当前水杯："+str(cupDis)) 
                time.sleep(0.3)
                lastDis=self.ultrasonic.distance()
                self.addLog("#当前距离水杯："+str(lastDis) )
                 
                self.goAhead(lastDis-200)
                
                # cupDis=self.checkObjectByLargeDegreeNew(-25,25,5,0.01,0)
                # self.goAhead(cupDis-170)
                
                self.putThingDownAndUp()
        else:          
          self.addLog("#4.2 在位置2上是红色")
          self.redPosition = 2
          self.bluePosition = 1
          self.turn(-28-25) 
          self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
          self.goAhead(self.task2_dis1 * -1)
          self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180+90)
          self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
          # self.ev3.speaker.play_file('jump.wav')
          self.paySound(['C4/8','D4/8','E4/8'])
          self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)               
          self.addLog("#4.2 转身面向水杯")
          self.turn(-33-5)
          self.goAhead(260)
          self.putThingDownAndUp()

        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group1 =round(time_interval,2)
        self.addLog("#goTask1_Group1 用时:"+ str(self.timeUsed_group1) )

    def goTask2_Group2(self):
        nowStart = time.time()
        self.addLog("GoTask1_Group2 开始")
        self.paySound(['F4/4'])
        self.addLog("#13 后退，找到蓝色")  
         
        if self.bluePosition == 2 :
            self.addLog("#14 蓝色在第二位置")  
            self.addLog("#14.1 回正")
            self.turn(self.turnOffDegree * -1)
            self.addLog("#14.2 转向蓝色")
            self.turn(20+15+10+10)
            
            self.addLog("#14.3 前进找蓝色")
            self.goAhead(30)
            self.addLog("#14.4 放下抓杆子")
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*14+90 )

            self.addLog("#14.5 扫描找蓝色")
            res,offset = self.scanColorByLargeDegree(Color.BLUE,-10,10,0.0625,8)
       
            self.addLog("#14.6 检查是蓝色后偏离角度："+str(offset))
            self.turnOffDegree = self.turnOffDegree + offset
            self.addLog("#14.7 当前总偏离角度："+str( self.turnOffDegree ))


            self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
            self.goAhead(self.task2_dis1 * -1)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180)
            self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
            # self.ev3.speaker.play_file('jump.wav')
            self.paySound(['C4/8','D4/8','E4/8'])
            self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)  
              
            self.addLog("#14.8 前进到中间")
            self.goAhead(self.task2_dis1+300)
            self.addLog("#14.9 回正")
            self.turn(self.turnOffDegree * -1)
        else:
            self.addLog("#14 蓝色在第一位置")  
            self.goAhead(-260)
            self.turn(33+5)
            self.goAhead(self.task2_dis1)
            self.turn(28)
            self.turn(5)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*(15-4) + 160)
            # self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
            self.goAhead(self.task2_dis1 * -1 + 20)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180+90)
            self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
            # self.ev3.speaker.play_file('jump.wav')
            self.paySound(['C4/8','D4/8','E4/8'])
            self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)    
            self.goAhead(self.task2_dis1)
            self.turn(2)
        
        self.addLog("#15 转身面向任务二")  
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group2 =round(time_interval,2)
        self.addLog("#goTask1_Group1 用时:"+ str(self.timeUsed_group2) )

    def goTask2_Group3(self):
        nowStart = time.time()
        MAX_LIMIT_DIS = 1000
        startDegree=-60
        endDegree=60
        stepDegree=21
        sleepTime=0.1
        offset=0
        count= 0
         
        cupDis=self.checkObjectByLargeDegreeNew2(startDegree,endDegree,stepDegree,sleepTime,offset)
        self.addLog("#goTask2_Group3 cupDis"+str(cupDis))
        self.goAhead(900)
        # self.goTask1_Group5()
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group3 =round(time_interval,2)
        self.addLog("#goTask2_Group3 用时:"+ str(self.timeUsed_group3) )

        

    def goTask2_Group4(self):
        nowStart = time.time()
        self.scanAndGo()
        self.putThingDownAndUp()
        self.robot.stop()
        # self.goTask1_Group5()
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group4 =round(time_interval,2)
        self.addLog("#goTask2_Group4 用时:"+ str(self.timeUsed_group4) )

    def goTask2(self):
        self.addLog("GoTask2 start---") 
        nowStart = time.time()   
        self.goTask2_Group1()
        self.goTask2_Group2()
        
        self.goTask2_Group3()
        self.goTask2_Group4()
        self.addLog("GoTask2 Finish---")
        # self.ev3.speaker.play_file('complete.wav')
        self.paySound(['C4/8','D4/8','E4/8','F4/8','G4/8','A4/8','B4/8','C5/4'])
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_groupTotal = round(time_interval,2)
        self.addLog("#GoTask2 总用时:"+ str(self.timeUsed_groupTotal)) 
        self.addLog("#        Group1:"+ str(self.timeUsed_group1)) 
        self.addLog("#        Group2:"+ str(self.timeUsed_group2))
        self.addLog("#        Group3:"+ str(self.timeUsed_group3))
        self.addLog("#        Group4:"+ str(self.timeUsed_group4))

    def goTask3_Group1(self):
        nowStart = time.time()
        res,offset= self.scanColorByLargeDegree(Color.RED,-18,18,0.0625,8)
        # while (not res):
        #     self.goAhead(40)
        #     res,offset= self.scanColorByLargeDegree(Color.RED,-18,18,0.125)

        print("goTask3_Group1 res:"+ str(res))
        print("goTask3_Group1 offset:"+ str(offset))

        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group1 =round(time_interval,2)
        self.addLog("#goTask1_Group1 用时:"+ str(self.timeUsed_group1) )

    def goTask3_Group2(self):
        nowStart = time.time()
        self.addLog("GoTask1_Group2 开始")
        self.paySound(['F4/4'])
        self.addLog("#13 后退，找到蓝色")  
        if self.bluePosition == 2 :
            self.addLog("#14 蓝色在第二位置")  
            self.goAhead(-140)
            self.turn(82)
            self.goAhead(self.task2_dis1)
            self.turn(-28)
            self.turn(-22-1-2)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*(15-4) + 160)
            # self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
            self.goAhead(self.task2_dis1 * -1 + 35)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180+90)
            self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
            # self.ev3.speaker.play_file('jump.wav')
            self.paySound(['C4/8','D4/8','E4/8'])
            self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)    
            self.goAhead(self.task2_dis1)
            self.turn(25-2)
        else:
            self.addLog("#14 蓝色在第一位置")  
            self.goAhead(-260)
            self.turn(33+5)
            self.goAhead(self.task2_dis1)
            self.turn(28)
            self.turn(5)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180*(15-4) + 160)
            # self.zhuaGanUpByDegree(self.EV3_zhuaGan_SPEED,180+180+180+180)
            self.goAhead(self.task2_dis1 * -1 + 20)
            self.zhuaGanDownByDegree(self.EV3_zhuaGan_SPEED,180+180+180+90)
            self.zhuaZiClose(self.EV3_zhuaZi_SPEED ,180*12+90+90)
            # self.ev3.speaker.play_file('jump.wav')
            self.paySound(['C4/8','D4/8','E4/8'])
            self.zhuaGanUpToTop(self.EV3_zhuaGan_SPEED)    
            self.goAhead(self.task2_dis1)
            self.turn(2)
        
        self.addLog("#15 转身面向任务二")  
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group2 =round(time_interval,2)
        self.addLog("#goTask1_Group1 用时:"+ str(self.timeUsed_group2) )

    def goTask3_Group3(self):
        nowStart = time.time()
        self.scanAndGo()
        self.putThingDownAndUp()
        self.robot.stop()
        # self.goTask1_Group5()
        nowEnd = time.time()
        time_interval = nowEnd - nowStart
        self.timeUsed_group3 =round(time_interval,2)
        self.addLog("#goTask2_Group2 用时:"+ str(self.timeUsed_group3) )

   